#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import serial
import re
import time
from DelphinROSv2.msg import altitude
from DelphinROSv2.msg import status

################################################################
def setUpSerial(): # set up the serial port
    global serialPort
    serialPort = serial.Serial(port='/dev/ttyS3', baudrate='9600', timeout=0.01) 
    serialPort.bytesize = serial.EIGHTBITS
    serialPort.stopbits = serial.STOPBITS_ONE
    serialPort.parity = serial.PARITY_NONE
    print "Initialised EchoSounder serial."
    print serialPort.portstr
    print serialPort.isOpen()
    return serialPort.isOpen()

################################################################
def validDataCheck():
    #Check string is of expected format ie of the form *.* m
    global serialPort
    
    while serialPort.inWaiting() <= 8:   
        pass
    while serialPort.inWaiting() > 8:                           # while there is data to be read - read the line
        data = serialPort.readline(size = None, eol = '\n')
        p = re.compile('\d+.\d+')                               # extract decimal number returned by the altimeter, throwing away the 'm' at the end of each line
        data = p.findall(data)
        if data==[]:
            string_status=False
        elif len(data)==1:
            string_status=True
        else:
            string_status=False            
        return string_status
################################################################
def listenForData():
    global serialPort
    
    while serialPort.inWaiting() > 8:                           # while there is data to be read - read the line
        data = serialPort.readline(size = None, eol = '\n')
        
        p = re.compile('\d+.\d+')                               # extract decimal number returned by the altimeter, throwing away the 'm' at the end of each line
        data = p.findall(data)
        data = float(data[0])
        
        pub.publish(altitude=data)
        
################################################################
def shutdown():
    serialPort.flushInput()
    serialPort.flushOutput()
    pubStatus.publish(nodeID = 3, status = False)
    serialPort.close()

        
################################################################        
#     INITIALISE     ###########################################
################################################################
if __name__ == '__main__':
    time.sleep(3) #Allow System to come Online    
    rospy.init_node('MicronEchoSounder')
    pub = rospy.Publisher('altimeter_out', altitude)
    pubStatus = rospy.Publisher('status', status)
    rospy.on_shutdown(shutdown)         #Defining shutdown behaviour  
    
    port_status = setUpSerial()

    str = "Altimeter port status = %s. Port = %s" %(port_status, serialPort.portstr)
    rospy.loginfo(str)
    time.sleep(0.3)
    string_status=validDataCheck()
    
    if (port_status and string_status) == True:    
        pubStatus.publish(nodeID = 3, status = True)
        rospy.loginfo("Altimeter online")
    else:
        pubStatus.publish(nodeID = 3, status = False)
    
    
    while not rospy.is_shutdown():
        listenForData()
        
        
#        global pubStatus
    
